#!/usr/bin/env python

import rospy
import time
import numpy as np
from math import pi, sqrt ,atan2
from std_srvs.srv import Empty
from geometry_msgs.msg import Twist, Point, Pose, Quaternion
import tf
from tf.transformations import euler_from_quaternion, quaternion_from_euler

kp_distance = 0.7
ki_distance = 0.002
kd_distance = 0.1

kp_angle = 3
ki_angle = 0.02
kd_angle = 0.5

goal_x = 1
goal_y = 1
class Combination():
    def __init__(self):
        rospy.init_node('Control_PID')
        self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        position = Point()
        move_cmd = Twist()
        rospy.on_shutdown(self.shutdown)
        self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
        r = rospy.Rate(50)
        self.tf_listener = tf.TransformListener()
        self.odom_frame = 'odom'


        try:
            self.tf_listener.waitForTransform(self.odom_frame, 'base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = 'base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, 'base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = 'base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between odom and base_link or base_footprint")
                rospy.signal_shutdown("tf Execption")
        self.control(goal_x,goal_y)

    def shutdown(self):
        self.cmd_vel.publish(Twist())
        rospy.wait_for_service('gazebo/reset_simulation')
        try:
            self.reset_proxy()
        except (rospy.ServiceException) as e:
            print("gazebo/reset_simulation service call failed")

    def control(self, x, y):

        (position, rotation) = self.get_odom()
        last_rotation = 0
        (goal_x, goal_y) = (float(x), float(y))

        goal_distance = sqrt(pow(goal_x - position.x, 2) + pow(goal_y - position.y, 2))
        now_distance = goal_distance

	move_cmd = Twist()
	r = rospy.Rate(50)

        previous_distance = 0
        total_distance = 0

        previous_angle = 0
        total_angle = 0

        while now_distance > 0.01 :
            x_start = position.x
            y_start = position.y
            path_angle = atan2(goal_y - y_start, goal_x - x_start)

            rot_angle =path_angle - rotation 
            if rot_angle > pi:
                rot_angle = rotation - 2 * pi
            elif rot_angle < -pi:
                rot_angle = rotation + 2 * pi

            diff_angle = rotation - previous_angle
            diff_distance = now_distance - previous_distance

            #control_signal_distance = kp_distance * now_distance + kd_distance * diff_distance
            #control_signal_angle = kp_angle * rot_angle + kd_angle * diff_angle
            control_signal_distance = (1 - abs(rot_angle/3.3))*kp_distance * now_distance + ki_distance * total_distance + kd_distance * diff_distance
            control_signal_angle = kp_angle * rot_angle + ki_angle * total_angle + kd_angle * diff_angle

            move_cmd.linear.x = min(control_signal_distance, 0.6)
            move_cmd.angular.z = control_signal_angle

            if move_cmd.angular.z > 0:
                move_cmd.angular.z = min(move_cmd.angular.z, 2.0)
            else:
                move_cmd.angular.z = max(move_cmd.angular.z, -2.0)

            self.cmd_vel.publish(move_cmd)
            previous_angle = rotation
            previous_distance = now_distance
            r.sleep()
            total_distance = total_distance + previous_distance
            (position, rotation) = self.get_odom()
            now_distance = sqrt(pow((goal_x - position.x), 2) + pow((goal_y - position.y), 2))

        (position, rotation) = self.get_odom()
        print("Current position and rotation are: ", (position.x, position.y, rotation))
        print("reached: ^_^")

        # rospy.loginfo("Stopping the robot ...")
        self.cmd_vel.publish(Twist())
        return
    def get_odom(self):
        try:
            (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
            rotation = euler_from_quaternion(rot)

        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return
        return (Point(*trans), rotation[2])
def main():
    time.sleep(0.1)
    try:
        combination = Combination()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()
